# Untitled - By: wuhuan - 周一 12月 6 2021
import math
import pyb
MPU_PWR_MGMT1_REG=0x6B
MPU_PWR_MGMT2_REG=0x6C
MPU_GYRO_CFG_REG=0x1B
MPU_ACCEL_CFG_REG=0x1C
MPU_SAMPLE_RATE_REG=0x19
MPU_CFG_REG=0x1A
MPU_DEVICE_ID_REG=0x75      #器件ID寄存器
MPU_INT_EN_REG=0x38
MPU_USER_CTRL_REG=0x6A      #用户控制寄存器
MPU_FIFO_EN_REG=0x23        #FIFO使能寄存器
MPU_INTBP_CFG_REG=0x37      #中断/旁路设置寄存器
MPU_DEVICE_ID_REG=0x75      #器件ID寄存器
MPU_ACCEL_YOUTH_REG=0X3D	#加速度值,Y轴高8位寄存器
MPU_ACCEL_ZOUTH_REG=0X3F	#加速度值,Z轴高8位寄存器


class MPU6050():

    def __init__(self,i2c,address=0x68):
        self.i2c=i2c
        self.address=address
        self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG,0x80)
        pyb.delay(100)
        self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG,0x00)
        self.MPU_Set_Gyro_Fsr(3)
        self.MPU_Set_Accel_Fsr(0)
        self.MPU_Set_Rate(50)
        self.Write_Mpu6050_REG(MPU_INT_EN_REG,0x00)
        self.Write_Mpu6050_REG(MPU_USER_CTRL_REG,0x00)
        self.Write_Mpu6050_REG(MPU_FIFO_EN_REG,0x00)
        self.Write_Mpu6050_REG(MPU_INTBP_CFG_REG,0x80)
        self.i2c.start()
        res=self.i2c.readfrom_mem(self.address,MPU_DEVICE_ID_REG,1)
        self.i2c.stop()
        if(res==0x68):
            self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG,0x01)
            self.Write_Mpu6050_REG(MPU_PWR_MGMT2_REG,0x00)
            self.MPU_Set_Rate(50)

    def Write_Mpu6050_REG(self,reg,data):
        buf=bytearray(1)
        buf[0]=data
        self.i2c.start()
        self.i2c.writeto_mem(self.address,reg,buf)
        self.i2c.stop()

    #fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
    def MPU_Set_Gyro_Fsr(self,fsr):
        return self.Write_Mpu6050_REG(MPU_GYRO_CFG_REG,fsr<<3)

    #fsr:0,±2g;1,±4g;2,±8g;3,±16g
    def MPU_Set_Accel_Fsr(self,fsr):
        return self.Write_Mpu6050_REG(MPU_ACCEL_CFG_REG,fsr<<3)
    def MPU_Set_LPF(self,lpf):
        if(lpf>=188):
            data=1
        elif(lpf>=98):
            data=2
        elif(lpf>=42):
            data=3
        elif(lpf>=20):
            data=4
        elif(lpf>=10):
            data=5
        else:
            data=6
        self.Write_Mpu6050_REG(MPU_CFG_REG,data)
    #rate:4~1000(Hz)
    def MPU_Set_Rate(self,rate):
        if(rate>1000):
            rate=1000
        if(rate<4):
            rate=4
        data=int(1000/rate-1)
        data=self.Write_Mpu6050_REG(MPU_SAMPLE_RATE_REG,data)
        return self.MPU_Set_LPF(rate/2)

    def get_raw_values(self,reg):
        self.i2c.start()
        a=self.i2c.readfrom_mem(self.address,reg,2)#获得reg寄存器8位和后面一个寄存器的8位
        self.i2c.stop()
        return a
    #将mpu6050寄存器的加速度值和陀螺仪值转化为有符号整数
    def bytes_toint(self,firstbyte,secondbyte):
        if not firstbyte & 0x80:
            return firstbyte<<8|secondbyte
        return (firstbyte-0x100)<<8|secondbyte

    def get_values(self):
        raw_Acy_bin=self.get_raw_values(MPU_ACCEL_YOUTH_REG)
        raw_Acz_bin=self.get_raw_values(MPU_ACCEL_ZOUTH_REG)
        Acy=self.bytes_toint(raw_Acy_bin[0],raw_Acy_bin[1])
        Acz=self.bytes_toint(raw_Acz_bin[0],raw_Acz_bin[1])
        angleAx=math.atan2((Acz/16384),(Acy/16384))*180/3.1415926+93.325
        return angleAx




